import time

import matplotlib.pyplot as plt
import math
import numpy

from AI.Astar.PointInPolygons import multi_polygon

MAX = 10000




class Node:
    def __init__(self, x, y):
        self.x = x
        self.y = y

    def is_equal(self, node):
        if isinstance(node, self.__class__):
            if node.x == self.x and node.y == self.y:
                return True
        return False


class Edge:
    def __init__(self, node1: Node, node2: Node):
        self.node1 = node1
        self.node2 = node2
        self.length = math.sqrt(math.pow(node1.x - node2.x, 2) + math.pow(node1.y - node2.y, 2))

    def is_intersect(self, edge):
        """
                判断该线段是否和目标线段相交
                这里假设自己是AB，目标线段是CD
                :param edge:
                :return:
                """
        if isinstance(edge, self.__class__):
            A = numpy.array([self.node1.x, self.node1.y])
            B = numpy.array([self.node2.x, self.node2.y])
            C = numpy.array([edge.node1.x, edge.node1.y])
            D = numpy.array([edge.node2.x, edge.node2.y])
            return (numpy.dot(numpy.cross((C - A), (D - A)), numpy.cross((C - B), (D - B))) < 0 and
                    numpy.dot(numpy.cross((A - C), (B - C)), numpy.cross((A - D), (B - D))) < 0
                    )

    def is_equal(self, edge):
        if isinstance(edge, self.__class__):
            if edge.node1.is_equal(self.node1) and edge.node2.is_equal(self.node2):
                return True
            if edge.node1.is_equal(self.node2) and edge.node2.is_equal(self.node1):
                return True
        return False


class Polygon:
    def __init__(self, edges):
        self.edges = edges
        self.nodes = []
        for edge in self.edges:
            if edge.node1 not in self.nodes:
                self.nodes.append(edge.node1)
            if edge.node2 not in self.nodes:
                self.nodes.append(edge.node2)


class Problem:
    def __init__(self, initial: Node, goal: Node, nodes, edges, polygons):
        """
        :param initial:起始点,
        :param goal: 重点
        :param nodes: 所有点的坐标
        :param edges: 所有障碍物边的坐标
        """
        self.nodes = nodes
        self.nodes.insert(0, initial)  # 起始点在邻接矩阵第0位
        self.nodes.insert(1, goal)  # 终点在邻接矩阵第1位
        self.state = 0  # 用来记录当前位置，初始为起点，也就是0
        self.goal = 1
        self.length = len(self.nodes)
        self.edges = edges
        self.polygons = polygons
        self.martix = self.creat_martix()
        self.martix[0][1] = MAX * 100  # 初始化要比普通的MAX还要大
        self.node_explored = [initial, ]
        self.path = [0, ]
        self.grade = 0

    def creat_martix(self):
        length = len(self.nodes)
        matrix = [[MAX for _ in range(length)] for _ in range(length)]
        for i in range(length):
            matrix[i][i] = 0
        for i in range(length):
            for j in range(i + 1, length):
                x = 0  # 判断两点之间的线段是否和其他线段相交，默认不相交
                y = 0  # 判断nodes[i]是否在该多边形中
                z = 0  # 判断nodes[j]是否在该多边形中
                for polygon in self.polygons:
                    # 这一步判断这两个点是否属于同一个多边形
                    for node in polygon.nodes:
                        if nodes[i].is_equal(node):
                            y = 1
                        if nodes[j].is_equal(node):
                            z = 1
                    if y == 1 and z == 1:
                        # 记录该多边形，之后用
                        polygon_mark = polygon
                        break
                    else:
                        y = 0
                        z = 0
                if y == 1 and z == 1:
                    # 如果是在相同多边形进一步判断是否属于邻点,默认不是邻点
                    for edge in polygon_mark.edges:
                        # print(nodes[i].x, nodes[i].y, "--", nodes[j].x, nodes[j].y)
                        # print(edge.node1.x,edge.node1.y,"--",edge.node2.x,edge.node2.y)
                        # print(Edge(nodes[i], nodes[j]).is_equal(edge))
                        # print(Edge(nodes[j], nodes[i]).is_equal(edge))
                        if Edge(nodes[i], nodes[j]).is_equal(edge) or Edge(nodes[j], nodes[i]).is_equal(edge):
                            # print(nodes[i].x, nodes[i].y, "--", nodes[j].x, nodes[j].y)
                            x = 1
                            matrix[i][j] = Edge(nodes[i], nodes[j]).length
                            matrix[j][i] = Edge(nodes[i], nodes[j]).length
                else:
                    for edge in edges:
                        if edge.is_intersect(Edge(nodes[i], nodes[j])):
                            x = 1
                    if x == 0:
                        matrix[i][j] = Edge(nodes[i], nodes[j]).length
                        matrix[j][i] = Edge(nodes[i], nodes[j]).length
        return matrix

    def run(self):
        """
        :return:返回路径的列表
        """
        while True:
            tem_grade = MAX * 100  # 初始化要比普通的MAX还要大
            if self.martix[self.state][1] < MAX:
                self.path.append(1)
                break
            for i in range(1, self.length):
                if i in self.path:
                    continue
                grade = self.grade + self.martix[self.state][i] + Edge(node1=nodes[i], node2=nodes[1]).length * 2

                if grade < tem_grade:
                    tem_grade = grade
                    tem_node = i
            if tem_grade < self.grade + self.martix[self.state][1]:
                self.grade = tem_grade
                self.state = tem_node
                self.path.append(tem_node)


nodes = [Node(44, 260), Node(211, 260), Node(211, 195), Node(44, 195),  # 横矩形
         Node(88, 10), Node(27, 78), Node(37, 146), Node(94, 163), Node(128, 75),  # 五边形
         Node(127, 169), Node(150, 64), Node(175, 169),  # 上三角
         Node(178, 98), Node(178, 16), Node(219, 10), Node(248, 48),  # 左四边形
         Node(219, 135), Node(235, 229), Node(270, 187),  # 下三角
         Node(327, 154), Node(257, 154), Node(258, 17), Node(327, 17),  # 纵矩形
         Node(371, 189), Node(336, 35), Node(361, 15), Node(382, 40),  # 右四边形
         Node(327, 265), Node(296, 240), Node(296, 192), Node(335, 161), Node(363, 192), Node(363, 243), ]  # 六边形
edges = [Edge(Node(44, 260), Node(211, 260)), Edge(Node(211, 260), Node(211, 195)), Edge(Node(211, 195), Node(44, 195)),
         Edge(Node(44, 260), Node(44, 195)),  # 横矩形
         Edge(Node(88, 10), Node(27, 78)), Edge(Node(27, 78), Node(37, 146)), Edge(Node(37, 146), Node(94, 163)),
         Edge(Node(94, 163), Node(128, 75)), Edge(Node(88, 10), Node(128, 75)),  # 五边形
         Edge(Node(127, 169), Node(150, 64)), Edge(Node(150, 64), Node(175, 169)), Edge(Node(127, 169), Node(175, 169)),
         # 上三角
         Edge(Node(178, 98), Node(178, 16)), Edge(Node(178, 16), Node(219, 10)), Edge(Node(219, 10), Node(248, 48)),
         Edge(Node(178, 98), Node(248, 48)),  # 左四边形
         Edge(Node(219, 135), Node(235, 229)), Edge(Node(235, 229), Node(270, 187)),
         Edge(Node(219, 135), Node(270, 187)),  # 下三角
         Edge(Node(327, 154), Node(257, 154)), Edge(Node(257, 154), Node(258, 17)), Edge(Node(258, 17), Node(327, 17)),
         Edge(Node(327, 154), Node(327, 17)),  # 纵矩形
         Edge(Node(371, 189), Node(336, 35)), Edge(Node(336, 35), Node(361, 15)), Edge(Node(361, 15), Node(382, 40)),
         Edge(Node(371, 189), Node(382, 40)),  # 右四边形
         Edge(Node(327, 265), Node(296, 240)), Edge(Node(296, 240), Node(296, 192)),
         Edge(Node(296, 192), Node(335, 161)),
         Edge(Node(335, 161), Node(363, 192)), Edge(Node(363, 192), Node(363, 243)),
         Edge(Node(327, 265), Node(363, 243))]  # 六边形

polygons = [Polygon(
    [Edge(Node(44, 260), Node(211, 260)), Edge(Node(211, 260), Node(211, 195)), Edge(Node(211, 195), Node(44, 195)),
     Edge(Node(44, 260), Node(44, 195))]),  # 横矩形
            Polygon([Edge(Node(88, 10), Node(27, 78)), Edge(Node(27, 78), Node(37, 146)),
                     Edge(Node(37, 146), Node(94, 163)),
                     Edge(Node(94, 163), Node(128, 75)), Edge(Node(88, 10), Node(128, 75))]),  # 五边形
            Polygon([Edge(Node(127, 169), Node(150, 64)), Edge(Node(150, 64), Node(175, 169)),
                     Edge(Node(127, 169), Node(175, 169))]),  # 上三角
            Polygon([Edge(Node(178, 98), Node(178, 16)), Edge(Node(178, 16), Node(219, 10)),
                     Edge(Node(219, 10), Node(248, 48)),
                     Edge(Node(178, 98), Node(248, 48))]),  # 左四边形
            Polygon([Edge(Node(219, 135), Node(235, 229)), Edge(Node(235, 229), Node(270, 187))
                        , Edge(Node(219, 135), Node(270, 187)), ]),  # 下三角
            Polygon([Edge(Node(327, 154), Node(257, 154)), Edge(Node(257, 154), Node(258, 17)),
                     Edge(Node(258, 17), Node(327, 17)),
                     Edge(Node(327, 154), Node(327, 17)), ]),  # 纵矩形
            Polygon([Edge(Node(371, 189), Node(336, 35)), Edge(Node(336, 35), Node(361, 15)),
                     Edge(Node(361, 15), Node(382, 40)),
                     Edge(Node(371, 189), Node(382, 40)), ]),  # 右四边形
            Polygon([Edge(Node(327, 265), Node(296, 240)), Edge(Node(296, 240), Node(296, 192)),
                     Edge(Node(296, 192), Node(335, 161)),
                     Edge(Node(335, 161), Node(363, 192)), Edge(Node(363, 192), Node(363, 243)),
                     Edge(Node(327, 265), Node(363, 243))])  # 六边形
            ]

initial_Node = Node(34.1, 215)
goal_Node = Node(436, 21)
problem = Problem(initial_Node, goal_Node, nodes, edges, polygons)

# 记录程序开始时间0
start_time = time.perf_counter()

pointIn = (initial_Node.x, initial_Node.y)
pointInPolygons = multi_polygon.is_inside(pointIn)
if pointInPolygons:
    print(f"是否在多边形的内部{multi_polygon.is_inside(pointIn)},没有正确路径")
else:
    problem.run()


# 记录程序结束时间
end_time = time.perf_counter()

# 计算程序运行时间
run_time = end_time - start_time

# 打印时间
print("程序运行时间：", run_time, "秒")

fig = plt.figure()
for edge in edges:
    plt.plot([edge.node1.x, edge.node2.x], [edge.node1.y, edge.node2.y], color="blue")
length = len(problem.path)
grade = 0
for i in range(length):
    if i == length - 1:
        break
    edge = Edge(node1=nodes[problem.path[i]], node2=nodes[problem.path[i + 1]])
    grade += edge.length
    plt.plot([edge.node1.x, edge.node2.x], [edge.node1.y, edge.node2.y], color="black")
plt.show()
print(grade)
print(problem.martix)
